/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#ifndef __IMU_TRACKING_H__
#define __IMU_TRACKING_H__

#include <deque>

#include "base/osa/osa++.h"

#include "AHRS_mini.h"

///
/// \brief The IMU_TrackingData class
///
class IMU_TrackingData
{
public:
    IMU_TrackingData() {
        clear();
    }

    IMU_TrackingData(AHRS_Frame &f) {
        clear();
        m_af = f;
    }

    virtual ~IMU_TrackingData() {
        return;
    }

    void clear(void) {
        for(int i=0; i<3; i++) {
            m_acc[i] = 0.0;
            m_vel[i] = 0.0;
            m_disp[i] = 0.0;
        }

        m_calculated = 0;

        m_af.clear();
    }

public:
    double      m_acc[3];       ///< Accelation
    double      m_vel[3];       ///< Velocity
    double      m_disp[3];      ///< Displacement

    AHRS_Frame  m_af;           ///< AHRS/IMU data

    int         m_calculated;   ///< flag indicating the frame is calculated or not
};


///
/// \brief The IMU_Tracking class
///
class IMU_Tracking
{
public:
    IMU_Tracking() {
        for(int i=0; i<3; i++) {
            m_initialVel[i]  = 0.0;
            m_initialDisp[i] = 0.0;
            m_disp[i]        = 0.0;
        }

        m_filt_a[0] = 1.000000000000000;        ///< filtfilt coefficients
        m_filt_a[1] = -0.996863331833438;
        m_filt_b[0] = 0.998431665916719;
        m_filt_b[1] = -0.998431665916719;

        m_g = 9.81;                             ///< gravity (m/s^2)
    }

    ~IMU_Tracking() {
    }

    ///
    /// \brief Set initial velocity
    ///
    /// \param vel - velocity data (x,y,z)
    ///
    void setInitialVel(double *vel) {
        for(int i=0; i<3; i++)
            m_initialVel[i] = vel[i];
    }

    ///
    /// \brief Set initial displacements
    ///
    /// \param disp - displacement (x, y, z)
    ///
    void setInitialDisp(double *disp) {
        for(int i=0; i<3; i++)
            m_initialDisp[i] = disp[i];
    }

    void integrateVel(void);
    void integrateDisp(void);

    int  frame_num(void);
    void frame_clear(void);
    void frame_insert(AHRS_Frame &f);

    int  get_disp(double *d);

protected:
    std::deque<IMU_TrackingData>    m_trackingDataQueue;        ///< data queue

    double                          m_initialVel[3];            ///< initial velocity
    double                          m_initialDisp[3];           ///< initial displacement

    double                          m_filt_a[2], m_filt_b[2];   ///< filtfilt coefficients
    double                          m_g;                        ///< gravity

    double                          m_disp[3];                  ///< current displacement

    pi::Mutex                       m_mux;
};

#endif // end of __IMU_TRACKING_H__
